

/***********************************
Function:
LP_Butterworth_SecondOrder.c

************************************/

#include " math.h"

//Notice that in order to simplify the derivation of the filter, you can safely set the sample time T to 2.
//1) Take analytical transfer function: H(s)
//2) Do bilinear transform: H( (z-1)/(z+1) )
//3) "Warp" cutoff frequency to find cutoff in the "bilinear domain"
//4) Express function as 

void getLPCoefficientsButterworth2Pole(const int samplerate, const double cutoff, double* const ax, double* const by)
{
    double PI      = 3.1415926535897932385;
    double sqrt2 = 1.4142135623730950488;

    double QcRaw  = (2 * PI * cutoff) / samplerate; // Find cutoff frequency in [0..PI]
    double QcWarp = tan(QcRaw); // Warp cutoff frequency

    double gain = 1 / (1+sqrt2/QcWarp + 2/(QcWarp*QcWarp));
    by[2] = (1 - sqrt2/QcWarp + 2/(QcWarp*QcWarp)) * gain;
    by[1] = (2 - 2 * 2/(QcWarp*QcWarp)) * gain;
    by[0] = 1;
    ax[0] = 1 * gain;
    ax[1] = 2 * gain;
    ax[2] = 1 * gain;
}

double xv[3];
double yv[3];

void filter(double* samples, int count)
{
   double ax[3];
   double by[3];

   getLPCoefficientsButterworth2Pole(100, 80, ax, by);

   for (int i=0;i<count;i++)
   {
       xv[2] = xv[1]; xv[1] = xv[0];
       xv[0] = samples[i];
       yv[2] = yv[1]; yv[1] = yv[0];

       yv[0] =   (ax[0] * xv[0] + ax[1] * xv[1] + ax[2] * xv[2]
                    - by[1] * yv[0]
                    - by[2] * yv[1]);

       samples[i] = yv[0];
   }
}